#include <path_planning/offline_processor.h>
#include <path_planning/path_planner.h>
#include <iostream>

using namespace std;

void OptionA(){
	OfflineProcessor* processor = new OfflineProcessor(2);
	processor->Init("localhost", 6665, 1);
	processor->SetTopologicalMap("topological_map_simple_room.txt");
	processor->Run("grid_map_generic_rooms.txt");
	processor->Clear();
	delete processor;
}

void OptionB(){
		PathPlanner *planner = new PathPlanner("grid_map_generic_rooms.txt", "topological_map_generic_rooms.txt");

		player_point_2d origin;
		origin.px=-2.80; origin.py=-5.30;

		player_point_2d goal;
		goal.px=14.44; goal.py=10.60;

		cout << "Requesting route from A to B"  << endl;
		WaypointList* list = planner->RequestWaypoints(origin, goal);


		//Printing Route
		player_point_2d* next_goal;
		next_goal = list->Next();
		cout << "TEST:Goto (" << next_goal->px << "; " << next_goal->py << ")" << endl;
		cout << "TEST:Reached (" << next_goal->px << "; " << next_goal->py << ")" << "!" << endl;

		next_goal = list->Next();
		cout << "TEST:Goto (" << next_goal->px << "; " << next_goal->py << ")" << endl;
		cout << "TEST:Reached (" << next_goal->px << "; " << next_goal->py << ")" << "!" << endl;
		//end Printing Route

		//Inform that there is a route obstruction
		player_point_2d* last_goal_reached = next_goal;
		next_goal = list->Next();
		cout << "TEST:Obstruction between (" << last_goal_reached->px << "; " << last_goal_reached->py << ") " << "and " <<
									"(" << next_goal->px << "; " << next_goal->py << ") " << endl;


		player_point_2d current;
		current.px = -12.91;
		current.py = 12.54;

		cout << "TEST:Call bug2" << endl;
		cout << "TEST:end of bug2. Current Position: (" << current.px << "; " << current.py << ")" << endl;

		delete list;
		list = planner->RequestAlternativeRoute(current, goal, *last_goal_reached, *next_goal);

		//printing route. At this time there is no obstruction until the end
		next_goal = list->Next();
		while(next_goal){
			cout << "Goto (" << next_goal->px << "; " << next_goal->py << ")" << endl;
			cout << "Reached (" << next_goal->px << "; " << next_goal->py << ")" << "!" << endl;
			next_goal = list->Next();
		}

		cout << "End of route" << endl;

}


void TESTPathPlanner(){

	// Let user choose which test to run.
	cout << "Enter a suboption:" << endl;
	cout << "a: Run offline processor [SCENARIO 1 - generic rooms]" << endl;
	cout << "b: Planning case.Fo from A=(-2.80; -5.30) to B=(14.44; 10.60) with route obstruction. [SCENARIO 1 - generic rooms ]" << endl;
	cout << ">>> ";
	char option;
	cin >> option;
	switch (option) {
		case 'a':
			OptionA();
			break;
		case 'b':
			OptionB();
			break;
		//TODO comitar outros testes
		default:
			cout << "Invalid option. Ending execution." << endl;
	}


}
